﻿using System;
using System.Collections.Generic;
using System.Text;
using RoverStudio.Math;
using FuzzyMarsRoverController;
using RoverStudio.Common.Controller;

namespace CaterpillarRover
{

    public class TankLineFollowerController
    {
        private FuzzyMarsRoverController.FuzzyMarsRoverController fuzzyMarsRoverController;
        private DateTime lastTimeStamp;
        private Rotation lastOrientation;

        public event ControlSignalChangedHandler ControlSignalChanged;

        //private Queue<Vector2D> lastPositions = new Queue<Vector2D>(10);
        //private double averageOfLastMovements;
        //private double powerAccRatio = 1.0;

        private Vector3D start;
        private Vector3D end;

        private double power = 0.25; //TODO: csinalni kell ezzel vmit..


        private double reverseDistance = 0.3; // m
        private bool reverseEnabled = false;

        private ITankController tankController;

        public ITankController TankController
        {
            get { return tankController; }
            set { tankController = value; }
        }


        public TankLineFollowerController(bool isWater)
        {
            this.fuzzyMarsRoverController = new FuzzyMarsRoverController.FuzzyMarsRoverController(isWater);

        }

        public void ContinueFollowing(Vector3D position, Rotation orientation, DateTime timeStamp)
        {
            if ((start == null) || (end == null))
                return;

            Vector2D start2D = (Vector2D)start;
            Vector2D end2D = (Vector2D)end;
            Vector2D position2D = (Vector2D)position;

            Vector2D directionToFollow = (end2D - start2D).GetNormalizedVector2D();
            Vector2D errorVector = position2D - (start2D + directionToFollow * (position2D - start2D) * directionToFollow);

            Vector2D pseudoTarget = end2D - errorVector;
            //Vector2D pseudoTarget = end2D;

            this.pseudoTarget = pseudoTarget;
            Vector2D pseudoTargetDirection = (pseudoTarget - position2D).GetNormalizedVector2D();

            Vector2D orientationY2D = ((Vector2D)orientation.YAxis).GetNormalizedVector2D();
            //double alpha = Math.Atan2(pseudoTargetDirection ^ orientationY2D, orientationY2D * pseudoTargetDirection);
            double alpha = -Math.Atan2(pseudoTargetDirection ^ orientationY2D, orientationY2D * pseudoTargetDirection);
            double omega = 0.0;


            if (lastOrientation != null)
            {
                omega = Math.Atan2((orientation.YAxis ^ lastOrientation.YAxis).Length(), orientation.YAxis * lastOrientation.YAxis)
                    / ((timeStamp - lastTimeStamp).TotalMilliseconds) * 1000.0;
            }

            lastOrientation = orientation;
            lastTimeStamp = timeStamp;

            //FuzzyControllerVaVdOutput output = fuzzyMarsRoverController.GetVaVdOutputs(alpha, omega);

            //float left = (float)(output.va - output.vd / 2.0) * 0.7F;
            //float right = (float)(output.va + output.vd / 2.0) * 0.7F;
            //caterpillarController.MoveTinyOS2(left, right, 0.5);


            //Console.WriteLine("Alpha: {0}   Omega: {1}  Left: {2}  Right:{3}", 180.0 / Math.PI * alpha, omega, left, right);
            double distance = (end2D - position2D).Length();
            if (reverseEnabled && distance < reverseDistance)
            {
                // tolatás?
                if (alpha > Math.PI / 2.0)
                {
                    alpha -= Math.PI;
                    distance = -distance;
                }
                if (alpha < -Math.PI / 2.0)
                {
                    alpha += Math.PI;
                    distance = -distance;
                }
            }
            FuzzyControllerVLeftVRightOutput output = fuzzyMarsRoverController.GetVLeftVRightOutputs(alpha, distance);


            //if (Math.Abs(output.vLeft) + Math.Abs(output.vRight) > 0.2)
            //{
            //    averageOfLastMovements = 0.0;
            //    Vector2D[] lastPositions = this.lastPositions.ToArray();
            //    Vector2D averagePosition = new Vector2D(0, 0);
            //    for (int i = 0; i < lastPositions.Length; i++)
            //    {
            //        averagePosition += lastPositions[i];
            //    }
            //    if (lastPositions.Length > 0)
            //    {
            //        averagePosition /= lastPositions.Length;
            //    }

            //    for (int i = 0; i < lastPositions.Length; i++)
            //    {
            //        averageOfLastMovements += (lastPositions[i] - averagePosition).Length();
            //    }
            //    if (lastPositions.Length > 0)
            //    {
            //        averageOfLastMovements /= lastPositions.Length;
            //    }

            //    if (averageOfLastMovements < 0.02)
            //    {
            //        powerAccRatio *= 1.02;
            //        powerAccRatio = Math.Min(4.0, powerAccRatio);
            //    }
            //    else
            //    {
            //        this.lastPositions.Clear();
            //        powerAccRatio = Math.Max(powerAccRatio * 0.7, 1.0);
            //    }
            //}
            //else
            //{
            //    powerAccRatio = 1.0;
            //}
            //double power = this.power * powerAccRatio;

            //this.lastPositions.Enqueue(position2D);
            //if (this.lastPositions.Count > 5)
            //    this.lastPositions.Dequeue();

            float vLeft = (float)(output.vLeft * power);
            float vRight = (float)(output.vRight * power);


            //if (Math.Max(Math.Abs(vLeft), Math.Abs(vRight)) > 0.9)
            //{
            //    vLeft = vLeft / Math.Max(Math.Abs(vLeft), Math.Abs(vRight)) * 0.9F;
            //    vRight = vRight / Math.Max(Math.Abs(vLeft), Math.Abs(vRight)) * 0.9F;
            //}

            if (this.ControlSignalChanged != null)
                ControlSignalChanged(this, new ControlSignalEventArgs(alpha, distance, vLeft, vRight, 0.5));


            tankController.MoveTank(vLeft, vRight, 0.5);
            Console.WriteLine("tank move: {0:0.00} {1:0.00}", output.vLeft, output.vRight);

        }

        public void Stop()
        {
            tankController.MoveTank(0.0, 0.0, 1.0);
        }

        private Vector3D pseudoTarget = new Vector3D();
        public Vector3D PseudoTarget
        {
            get { return pseudoTarget; }
        }

        internal void SetLineToFollow(Vector3D start, Vector3D end)
        {
            this.start = start;
            this.end = end;
        }
    }
}
